/**
 * @file:          ThreeLoopPidFocV1_0.c
 * @brief:
 * @details:
 * @author:        wjh
 * @date created:  2023.06.14
 * @version:       1.0.0
 * @par copyright (c):
 *
 * @par history (desc):
 *   version:1.0.0, wjh, 2023.06.14,00:34:56
 */

/* Include Files **************************************************************/
#include "ThreeLoopPidFocV1_0.h"
#include "common.h"

/* Global Variable Define *****************************************************/

/* Function Define ************************************************************/

uint8 ThreeLoopPidFocV1_0_Init(struct ThreeLoopPidFocV1_0 *self)
{
    self->UpdateParam = ThreeLoopPidFocV1_0_UpdateParam;

    self->UpdateParam(self);
    // self->iq_loop_pid.Init = self->id_loop_pid.Init;
    // self->iq_loop_pid.param_en = self->id_loop_pid.param_en;
    // self->iq_loop_pid.param_bypass = self->id_loop_pid.param_bypass;
    // self->iq_loop_pid.param_T = self->id_loop_pid.param_T;
    // self->iq_loop_pid.param_pid_mode = self->id_loop_pid.param_pid_mode;
    // self->iq_loop_pid.param_limit_ref = self->id_loop_pid.param_limit_ref;
    // self->iq_loop_pid.param_limit_ui = self->id_loop_pid.param_limit_ui;
    // self->iq_loop_pid.parma_limit_out = self->id_loop_pid.parma_limit_out;
    // self->iq_loop_pid.param_kp = self->id_loop_pid.param_kp;
    // self->iq_loop_pid.param_ki = self->id_loop_pid.param_ki;
    // self->iq_loop_pid.param_kd = self->id_loop_pid.param_kd;
    // self->iq_loop_pid.param_kc = self->id_loop_pid.param_kc;
    // self->iq_loop_pid.ud_filter.Init = self->id_loop_pid.ud_filter.Init;
    // self->iq_loop_pid.ud_filter.param_en = self->id_loop_pid.ud_filter.param_en;
    // self->iq_loop_pid.ud_filter.param_Ts = self->id_loop_pid.ud_filter.param_Ts;
    // self->iq_loop_pid.ud_filter.param_fc = self->id_loop_pid.ud_filter.param_fc;
    // self->iq_loop_pid.fal.Init = self->id_loop_pid.fal.Init;
    // self->iq_loop_pid.fal.param_en = self->id_loop_pid.fal.param_en;
    // self->iq_loop_pid.fal.param_a = self->id_loop_pid.fal.param_a;
    // self->iq_loop_pid.fal.param_d = self->id_loop_pid.fal.param_d;
    // self->iq_loop_pid.fal.param_zoom = self->id_loop_pid.fal.param_zoom;
    // self->iq_loop_pid.fal.param_section_nums = self->id_loop_pid.fal.param_section_nums;
    // self->iq_loop_pid.fal.param_inteval_gain = self->id_loop_pid.fal.param_inteval_gain;

    // self->ibeta_filter.Init = self->ialpha_filter.Init;
    // self->ibeta_filter.param_en = self->ialpha_filter.param_en;
    // self->ibeta_filter.param_Ts = self->ialpha_filter.param_Ts;
    // self->ibeta_filter.param_fc = self->ialpha_filter.param_fc;

    self->fhan_controller.Init != NULL ? self->fhan_controller.Init(&self->fhan_controller) : 0;
    self->pos_loop_pid.Init(&self->pos_loop_pid);
    self->spd_loop_pid.Init(&self->spd_loop_pid);
    self->id_loop_pid.Init(&self->id_loop_pid);
    self->iq_loop_pid.Init(&self->iq_loop_pid);
    self->leso.Init(&self->leso);
    self->fwc.Init(&self->fwc);

    // self->spd_fdb_filter_moving_average.Init(&self->spd_fdb_filter_moving_average);

    self->speed_filter.Init(&self->speed_filter);
    self->ialpha_filter.Init(&self->ialpha_filter);
    self->ibeta_filter.Init(&self->ibeta_filter);

    for (uint16 i = 0; i < FILTER_NOTCH_NUM; i++)
    {
        self->spd_out_filter_notch[i].Init(&self->spd_out_filter_notch[i]);
    }

    self->spd_out_filter_binary_second_order.Init(&self->spd_out_filter_binary_second_order);
    self->z3_filter_binary_second_order.Init(&self->z3_filter_binary_second_order);

    // self->motor_ripple_compensator.Init(&self->motor_ripple_compensator);

    self->foc.Init(&self->foc);

    self->td.Init(&self->td);

    return TRUE;
}

void ThreeLoopPidFocV1_0_SeriesPidLeso_Run(struct ThreeLoopPidFocV1_0 *self)
{
    // DISABLE_GLOBAL_INTERRUPT();
    // GPIO_WriteBit(GPIOB, GPIO_Pin_3, SET);

    /* leso */
    // self->leso.in_u = self->spd_loop_pid.out - self->leso.param_z3k * self->leso.out_z3 / self->leso.param_b0;
    // self->leso.in_u = self->spd_loop_pid.out - self->iqfdf_before_u;
    self->leso.in_u = self->spd_loop_pid.out + self->iqfdf_before_u;
    self->leso.in_y = self->in_pos_fdb;
    self->leso.Run(&self->leso);
    self->leso.out_z3 = self->z3_filter_binary_second_order.Run(&self->z3_filter_binary_second_order, self->leso.out_z3);
    self->iqfdf_before_u = (self->in_acc_cmd - self->leso.param_z3k * self->leso.out_z3) / self->leso.param_b0;

    self->speed_filter.Run(&self->speed_filter, self->in_spd_fdb);
    self->pos_fdb_ctrl = *self->pos_fdb_ctrl_link;
    self->spd_fdb_ctrl = *self->spd_fdb_ctrl_link;

    /* acquire id iq fdb*/
    self->foc.ialpha = self->ialpha_filter.Run(&self->ialpha_filter, self->in_ialpha_fdb);
    self->foc.ibeta = self->ibeta_filter.Run(&self->ibeta_filter, self->in_ibeta_fdb);

    /* get foc elec theta fdb */
    if (self->param_control_mode == CONTROL_MODE_OPEN_LOOP_ELEC_ANGLE_FREE_CONTROL)
    {
        self->in_elec_angle_fdb = 0;
    }
    self->foc.theta = self->in_elec_angle_fdb + self->in_custom_elecangle_cmd;

    /* park transform */
    self->foc.Park(self->foc.ialpha,
                   self->foc.ibeta,
                   self->foc.theta,
                   &self->foc.id,
                   &self->foc.iq);

    /* pid controller */
    if (self->param_en_all_control_switch == 1)
    {
        /* position loop pid control */
        // self->pos_loop_pid.in_ref = self->in_pos_cmd + self->in_custom_pos_cmd;
        self->pos_loop_pid.in_ref = Limit(self->in_pos_cmd + self->in_custom_pos_cmd, -self->pos_loop_pid.param_limit_ref, self->pos_loop_pid.param_limit_ref);

        self->pos_loop_pid.in_fdb = self->pos_fdb_ctrl;

        // self->pos_loop_pid.in_fdb = fmodf(self->pos_loop_pid.in_fdb, BM_2PI);

        if (self->param_single_turn_mode != 0)
        {
            float err = self->pos_loop_pid.in_ref - self->pos_loop_pid.in_fdb;
            if (err > BM_PI)
            {
                self->pos_loop_pid.in_fdb += BM_2PI;
            }
            else if (err < -BM_PI)
            {
                self->pos_loop_pid.in_ref += BM_2PI;
            }
        }

        self->pos_loop_pid.Run(&self->pos_loop_pid);

        /* spd feedforward */
        if (self->param_en_feedforward_speed)
        {
            self->spd_feedforward = self->in_spd_cmd;
            self->spd_feedforward = Limit(self->spd_feedforward, -self->param_feedforward_speed_limit, self->param_feedforward_speed_limit);
        }
        else
        {
            self->spd_feedforward = 0;
        }

        /* speed loop pid control */
        // self->spd_loop_pid.in_ref = 0 + self->pos_loop_pid.out + self->spd_feedforward + self->in_custom_spd_cmd;
        self->spd_loop_pid.in_ref = Limit(0 + self->pos_loop_pid.out + self->spd_feedforward + self->in_custom_spd_cmd, -self->spd_loop_pid.param_limit_ref, self->spd_loop_pid.param_limit_ref);

        self->spd_loop_pid.in_fdb = self->spd_fdb_ctrl;
        self->spd_loop_pid.Run(&self->spd_loop_pid);

        /* spd out filter */
        for (uint16 i = 0; i < FILTER_NOTCH_NUM; i++)
        {
            self->spd_loop_pid.out = self->spd_out_filter_notch[i].Run(&self->spd_out_filter_notch[i], self->spd_loop_pid.out);
        }
        self->spd_loop_pid.out = self->spd_out_filter_binary_second_order.Run(&self->spd_out_filter_binary_second_order, self->spd_loop_pid.out);

        /* current feedforward */
        if (self->param_en_feedforward_idiq)
        {
            self->iq_feedforward = self->iqfdf_before_u; // +
                                                         // self->motor_ripple_compensator.Run(&self->motor_ripple_compensator, self->in_pos_fdb_abs);

            self->iq_feedforward = Limit(self->iq_feedforward, -self->param_feedforward_idiq_limit, self->param_feedforward_idiq_limit);
        }
        else
        {
            self->id_feedforward = 0;
            self->iq_feedforward = 0;
        }

        /* field weakening control */
        self->fwc.in_iq_ref = 0.0 + self->spd_loop_pid.out + self->iq_feedforward;
        // self->fwc.in_wm = self->in_spd_cmd;
        self->fwc.Run(&self->fwc);

        /* id loop pid control */
        // self->id_loop_pid.in_ref = 0.0 + self->id_feedforward;
        // self->id_loop_pid.in_ref = 0.0 + self->fwc.out_id_ref + self->id_feedforward + self->in_custom_id_cmd;
        self->id_loop_pid.in_ref = Limit(0.0 + self->fwc.out_id_ref + self->id_feedforward + self->in_custom_id_cmd, -self->id_loop_pid.param_limit_ref, self->id_loop_pid.param_limit_ref);

        // self->id_loop_pid.in_ref = 2.0*sin_bm(BM_2PI*261*tick*3.0e-5);

        self->id_loop_pid.in_fdb = self->foc.id;
        self->id_loop_pid.Run(&self->id_loop_pid);

        /* iq loop pid control */
        // self->iq_loop_pid.in_ref = 0.0 + self->spd_loop_pid.out + self->iq_feedforward;
        // self->iq_loop_pid.in_ref = 0.0 + self->fwc.out_iq_ref + self->in_custom_iq_cmd;
        self->iq_loop_pid.in_ref = Limit(0.0 + self->fwc.out_iq_ref + self->in_custom_iq_cmd, -self->iq_loop_pid.param_limit_ref, self->iq_loop_pid.param_limit_ref);

        // self->iq_loop_pid.in_ref = 2.0*sin_bm(BM_2PI*261*tick*3.0e-5);

        self->iq_loop_pid.in_fdb = self->foc.iq;
        self->iq_loop_pid.Run(&self->iq_loop_pid);

        /* uduq feedforward */
        if (self->param_en_feedforward_uduq)
        {
            self->motor->Vbemf = self->motor->param_Ke * (*self->speed_source);
            // self->motor->Vbemf = 0;
            self->ud_feedforward = (self->param_motor_R * self->id_loop_pid.in_ref -
                                    (*self->speed_source) * self->motor->param_pole_pair * self->param_motor_L * self->iq_loop_pid.in_ref) /
                                   self->motor->param_power_supply;
            self->uq_feedforward = (self->param_motor_R * self->iq_loop_pid.in_ref +
                                    (*self->speed_source) * self->motor->param_pole_pair * self->param_motor_L * self->id_loop_pid.in_ref + self->motor->Vbemf) /
                                   self->motor->param_power_supply;

            self->ud_feedforward = Limit(self->ud_feedforward, -self->param_feedforward_uduq_limit, self->param_feedforward_uduq_limit);
            self->uq_feedforward = Limit(self->uq_feedforward, -self->param_feedforward_uduq_limit, self->param_feedforward_uduq_limit);
        }
        else
        {
            self->ud_feedforward = 0;
            self->uq_feedforward = 0;
        }

        /* foc ud uq */
        self->foc.ud = Limit(self->id_loop_pid.out + self->ud_feedforward + self->in_ud_cmd + self->in_custom_ud_cmd, -1, 1);
        self->foc.uq = Limit(self->iq_loop_pid.out + self->uq_feedforward + self->in_uq_cmd + self->in_custom_uq_cmd, -1, 1);

        /* test */
        // static uint32 tick = 0;
        // tick++;

        // self->foc.ud = 0.0;
        // self->foc.uq = -0.1;
        // self->foc.theta = 0.0;

        /* ipark */
        self->foc.IPark(self->foc.ud,
                        self->foc.uq,
                        self->foc.theta,
                        &self->foc.ualpha,
                        &self->foc.ubeta);

        // self->out_u_A = self->foc.ualpha;
        // self->out_u_B = self->foc.ubeta;
    }
    // GPIO_WriteBit(GPIOB, GPIO_Pin_3, RESET);
    // ENABLE_GLOBAL_INTERRUPT();
}

void ThreeLoopPidFocV1_0_SeriesPidLeso_Clear(struct ThreeLoopPidFocV1_0 *self)
{
    self->pos_loop_pid.Clear(&self->pos_loop_pid);
    self->spd_loop_pid.Clear(&self->spd_loop_pid);
    self->id_loop_pid.Clear(&self->id_loop_pid);
    self->iq_loop_pid.Clear(&self->iq_loop_pid);

    // self->spd_fdb_filter_moving_average.InitBuff(&self->spd_fdb_filter_moving_average, 0);
    self->speed_filter.Clear(&self->speed_filter);
    self->ialpha_filter.Clear(&self->ialpha_filter);
    self->ibeta_filter.Clear(&self->ibeta_filter);

    for (uint16 i = 0; i < FILTER_NOTCH_NUM; i++)
    {
        self->spd_out_filter_notch[i].Clear(&self->spd_out_filter_notch[i]);
    }
    self->spd_out_filter_binary_second_order.Clear(&self->spd_out_filter_binary_second_order);

    self->leso.Init_z1z2z3(&self->leso, self->in_pos_fdb, 0, 0);
}

void ThreeLoopPidFocV1_0_ParallelPidLeso_Run(struct ThreeLoopPidFocV1_0 *self)
{
    /* leso */
    self->leso.in_u = self->spd_loop_pid.out + self->iqfdf_before_u;
    self->leso.in_y = self->in_pos_fdb;
    self->leso.Run(&self->leso);
    self->leso.out_z3 = self->z3_filter_binary_second_order.Run(&self->z3_filter_binary_second_order, self->leso.out_z3);
    self->iqfdf_before_u = (self->in_acc_cmd - self->leso.param_z3k * self->leso.out_z3) / self->leso.param_b0;

    /* acquire spd */
    // self->spd_fdb_filter_moving_average.Run(&self->spd_fdb_filter_moving_average, self->in_spd_fdb);
    self->speed_filter.Run(&self->speed_filter, self->in_spd_fdb);
    self->pos_fdb_ctrl = *self->pos_fdb_ctrl_link;
    self->spd_fdb_ctrl = *self->spd_fdb_ctrl_link;

    /* acquire id iq fdb*/
    self->foc.ialpha = self->ialpha_filter.Run(&self->ialpha_filter, self->in_ialpha_fdb);
    self->foc.ibeta = self->ibeta_filter.Run(&self->ibeta_filter, self->in_ibeta_fdb);

    /* get foc elec theta fdb */
    self->foc.theta = self->in_elec_angle_fdb;

    /* park transform */
    self->foc.Park(self->foc.ialpha,
                   self->foc.ibeta,
                   self->foc.theta,
                   &self->foc.id,
                   &self->foc.iq);

    /* pid controller */
    if (self->param_en_all_control_switch == 1)
    {
        /* position loop pid control */
        self->pos_loop_pid.in_ref = self->in_pos_cmd;
        self->pos_loop_pid.in_fdb = self->pos_fdb_ctrl;

        // self->pos_loop_pid.in_fdb = fmodf(self->pos_loop_pid.in_fdb, BM_2PI);
        if (self->param_single_turn_mode != 0)
        {
            float err = self->pos_loop_pid.in_ref - self->pos_loop_pid.in_fdb;

            if (err > BM_PI)
            {
                self->pos_loop_pid.in_fdb += BM_2PI;
            }
            else if (err < -BM_PI)
            {
                self->pos_loop_pid.in_ref += BM_2PI;
            }
        }

        self->pos_loop_pid.Run(&self->pos_loop_pid);

        /* speed loop pid control */
        self->spd_loop_pid.in_ref = self->in_spd_cmd;
        self->spd_loop_pid.in_fdb = self->spd_fdb_ctrl;
        self->spd_loop_pid.Run(&self->spd_loop_pid);

        /* pid total output */
        self->spd_loop_pid.out += self->pos_loop_pid.out;

        /* spd out filter */
        for (uint16 i = 0; i < FILTER_NOTCH_NUM; i++)
        {
            self->spd_loop_pid.out = self->spd_out_filter_notch[i].Run(&self->spd_out_filter_notch[i], self->spd_loop_pid.out);
        }
        self->spd_loop_pid.out = self->spd_out_filter_binary_second_order.Run(&self->spd_out_filter_binary_second_order, self->spd_loop_pid.out);

        /* current feedforward */
        if (self->param_en_feedforward_idiq)
        {
            self->iq_feedforward = self->iqfdf_before_u; // +
                                                         // self->motor_ripple_compensator.Run(&self->motor_ripple_compensator, self->in_pos_fdb_abs);

            self->iq_feedforward = Limit(self->iq_feedforward, -self->param_feedforward_idiq_limit, self->param_feedforward_idiq_limit);
        }
        else
        {
            self->id_feedforward = 0;
            self->iq_feedforward = 0;
        }

        /* field weakening control */
        self->fwc.in_iq_ref = 0.0 + self->spd_loop_pid.out + self->iq_feedforward;
        self->fwc.in_wm = self->in_spd_cmd;
        self->fwc.Run(&self->fwc);

        /* id loop pid control */
        // self->id_loop_pid.in_ref = 0.0 + self->id_feedforward;
        self->id_loop_pid.in_ref = 0.0 + self->fwc.out_id_ref + self->id_feedforward;
        // self->id_loop_pid.in_ref = 2.0*sin_bm(BM_2PI*261*tick*3.0e-5);

        self->id_loop_pid.in_fdb = self->foc.id;
        self->id_loop_pid.Run(&self->id_loop_pid);

        /* iq loop pid control */
        // self->iq_loop_pid.in_ref = 0.0 + self->spd_loop_pid.out + self->iq_feedforward;
        self->iq_loop_pid.in_ref = 0.0 + self->fwc.out_iq_ref;
        // self->iq_loop_pid.in_ref = 2.0*sin_bm(BM_2PI*261*tick*3.0e-5);

        self->iq_loop_pid.in_fdb = self->foc.iq;
        self->iq_loop_pid.Run(&self->iq_loop_pid);

        /* uduq feedforward */
        if (self->param_en_feedforward_uduq)
        {
            self->motor->Vbemf = self->motor->param_Ke * (*self->speed_source);
            // self->motor->Vbemf = 0;
            self->ud_feedforward = (self->param_motor_R * self->id_loop_pid.in_ref -
                                    (*self->speed_source) * self->motor->param_pole_pair * self->param_motor_L * self->iq_loop_pid.in_ref) /
                                   self->motor->param_power_supply;
            self->uq_feedforward = (self->param_motor_R * self->iq_loop_pid.in_ref +
                                    (*self->speed_source) * self->motor->param_pole_pair * self->param_motor_L * self->id_loop_pid.in_ref + self->motor->Vbemf) /
                                   self->motor->param_power_supply;

            self->ud_feedforward = Limit(self->ud_feedforward, -self->param_feedforward_uduq_limit, self->param_feedforward_uduq_limit);
            self->uq_feedforward = Limit(self->uq_feedforward, -self->param_feedforward_uduq_limit, self->param_feedforward_uduq_limit);
        }
        else
        {
            self->ud_feedforward = 0;
            self->uq_feedforward = 0;
        }

        /* foc ud uq */
        self->foc.ud = Limit(self->id_loop_pid.out + self->ud_feedforward + self->in_ud_cmd, -1, 1);
        self->foc.uq = Limit(self->iq_loop_pid.out + self->uq_feedforward + self->in_uq_cmd, -1, 1);

        /* test */
        // static uint32 tick = 0;
        // tick++;

        // self->foc.ud = 0.2*(sin_bm(BM_2PI*261*3*tick*3.0e-5)>0?1:0);
        // self->foc.uq = 0;
        // self->foc.theta = 0.0;

        /* ipark */
        self->foc.IPark(self->foc.ud,
                        self->foc.uq,
                        self->foc.theta,
                        &self->foc.ualpha,
                        &self->foc.ubeta);

        // self->out_u_A = self->foc.ualpha;
        // self->out_u_B = self->foc.ubeta;
    }
}

void ThreeLoopPidFocV1_0_ParallelPidLeso_Clear(struct ThreeLoopPidFocV1_0 *self)
{
    self->pos_loop_pid.Clear(&self->pos_loop_pid);
    self->spd_loop_pid.Clear(&self->spd_loop_pid);
    self->id_loop_pid.Clear(&self->id_loop_pid);
    self->iq_loop_pid.Clear(&self->iq_loop_pid);

    // self->spd_fdb_filter_moving_average.InitBuff(&self->spd_fdb_filter_moving_average, 0);
    self->speed_filter.Clear(&self->speed_filter);
    self->ialpha_filter.Clear(&self->ialpha_filter);
    self->ibeta_filter.Clear(&self->ibeta_filter);

    for (uint16 i = 0; i < FILTER_NOTCH_NUM; i++)
    {
        self->spd_out_filter_notch[i].Clear(&self->spd_out_filter_notch[i]);
    }
    self->spd_out_filter_binary_second_order.Clear(&self->spd_out_filter_binary_second_order);

    self->leso.Init_z1z2z3(&self->leso, self->in_pos_fdb, 0, 0);
}

void ThreeLoopPidFocV1_0_FhanLeso_Run(struct ThreeLoopPidFocV1_0 *self)
{
    // DISABLE_GLOBAL_INTERRUPT();
    // GPIO_WriteBit(GPIOB, GPIO_Pin_3, SET);

    /* leso */
    self->leso.in_u = self->spd_loop_pid.out + self->iqfdf_before_u;
    self->leso.in_y = self->in_pos_fdb;
    self->leso.Run(&self->leso);
    self->leso.out_z3 = self->z3_filter_binary_second_order.Run(&self->z3_filter_binary_second_order, self->leso.out_z3);
    self->iqfdf_before_u = (self->in_acc_cmd - self->leso.param_z3k * self->leso.out_z3) / self->leso.param_b0;

    /* acquire spd */
    // self->spd_fdb_filter_moving_average.Run(&self->spd_fdb_filter_moving_average, self->in_spd_fdb);
    self->speed_filter.Run(&self->speed_filter, self->in_spd_fdb);
    self->pos_fdb_ctrl = *self->pos_fdb_ctrl_link;
    self->spd_fdb_ctrl = *self->spd_fdb_ctrl_link;

    /* acquire id iq fdb*/
    self->foc.ialpha = self->ialpha_filter.Run(&self->ialpha_filter, self->in_ialpha_fdb);
    self->foc.ibeta = self->ibeta_filter.Run(&self->ibeta_filter, self->in_ibeta_fdb);

    /* get foc elec theta fdb */
    self->foc.theta = self->in_elec_angle_fdb;

    /* park transform */
    self->foc.Park(self->foc.ialpha,
                   self->foc.ibeta,
                   self->foc.theta,
                   &self->foc.id,
                   &self->foc.iq);

    /* controller */
    if (self->param_en_all_control_switch == 1)
    {
        /* pos spd fhan control */
        self->fhan_controller.in_x1 = self->in_pos_cmd - self->pos_fdb_ctrl;
        self->fhan_controller.in_x2 = self->param_fhan_controller_c * (self->in_spd_cmd - self->spd_fdb_ctrl);
        self->fhan_controller.Run(&self->fhan_controller);

        self->spd_loop_pid.out = -self->fhan_controller.out_u;

        // /* position loop pid control */
        // self->pos_loop_pid.in_ref = self->in_pos_cmd;
        // self->pos_loop_pid.in_fdb = self->pos_fdb_ctrl;
        // self->pos_loop_pid.Run(&self->pos_loop_pid);

        // /* spd feedforward */
        // if (self->param_en_feedforward_speed)
        // {
        //     self->spd_feedforward = self->in_spd_cmd;
        //     self->spd_feedforward = Limit(self->spd_feedforward, -self->param_feedforward_speed_limit, self->param_feedforward_speed_limit);
        // }
        // else
        // {
        //     self->spd_feedforward = 0;
        // }

        // /* speed loop pid control */
        // self->spd_loop_pid.in_ref = 0 + self->pos_loop_pid.out + self->spd_feedforward;
        // self->spd_loop_pid.in_fdb = self->spd_fdb_ctrl;
        // self->spd_loop_pid.Run(&self->spd_loop_pid);

        /* spd out filter */
        for (uint16 i = 0; i < FILTER_NOTCH_NUM; i++)
        {
            self->spd_loop_pid.out = self->spd_out_filter_notch[i].Run(&self->spd_out_filter_notch[i], self->spd_loop_pid.out);
        }
        self->spd_loop_pid.out = self->spd_out_filter_binary_second_order.Run(&self->spd_out_filter_binary_second_order, self->spd_loop_pid.out);

        /* current feedforward */
        if (self->param_en_feedforward_idiq)
        {
            self->iq_feedforward = self->iqfdf_before_u; // +
                                                         // self->motor_ripple_compensator.Run(&self->motor_ripple_compensator, self->in_pos_fdb_abs);

            self->iq_feedforward = Limit(self->iq_feedforward, -self->param_feedforward_idiq_limit, self->param_feedforward_idiq_limit);
        }
        else
        {
            self->id_feedforward = 0;
            self->iq_feedforward = 0;
        }

        /* field weakening control */
        self->fwc.in_iq_ref = 0.0 + self->spd_loop_pid.out + self->iq_feedforward;
        self->fwc.in_wm = self->in_spd_cmd;
        self->fwc.Run(&self->fwc);

        /* id loop pid control */
        // self->id_loop_pid.in_ref = 0.0 + self->id_feedforward;
        self->id_loop_pid.in_ref = 0.0 + self->fwc.out_id_ref + self->id_feedforward;
        // self->id_loop_pid.in_ref = 2.0*sin_bm(BM_2PI*261*tick*3.0e-5);

        self->id_loop_pid.in_fdb = self->foc.id;
        self->id_loop_pid.Run(&self->id_loop_pid);

        /* iq loop pid control */
        // self->iq_loop_pid.in_ref = 0.0 + self->spd_loop_pid.out + self->iq_feedforward;
        self->iq_loop_pid.in_ref = 0.0 + self->fwc.out_iq_ref;
        // self->iq_loop_pid.in_ref = 2.0*sin_bm(BM_2PI*261*tick*3.0e-5);

        self->iq_loop_pid.in_fdb = self->foc.iq;
        self->iq_loop_pid.Run(&self->iq_loop_pid);

        /* uduq feedforward */
        if (self->param_en_feedforward_uduq)
        {
            self->motor->Vbemf = self->motor->param_Ke * (*self->speed_source);
            // self->motor->Vbemf = 0;
            self->ud_feedforward = (self->param_motor_R * self->id_loop_pid.in_ref -
                                    (*self->speed_source) * self->motor->param_pole_pair * self->param_motor_L * self->iq_loop_pid.in_ref) /
                                   self->motor->param_power_supply;
            self->uq_feedforward = (self->param_motor_R * self->iq_loop_pid.in_ref +
                                    (*self->speed_source) * self->motor->param_pole_pair * self->param_motor_L * self->id_loop_pid.in_ref + self->motor->Vbemf) /
                                   self->motor->param_power_supply;

            self->ud_feedforward = Limit(self->ud_feedforward, -self->param_feedforward_uduq_limit, self->param_feedforward_uduq_limit);
            self->uq_feedforward = Limit(self->uq_feedforward, -self->param_feedforward_uduq_limit, self->param_feedforward_uduq_limit);
        }
        else
        {
            self->ud_feedforward = 0;
            self->uq_feedforward = 0;
        }

        /* foc ud uq */
        self->foc.ud = Limit(self->id_loop_pid.out + self->ud_feedforward + self->in_ud_cmd, -1, 1);
        self->foc.uq = Limit(self->iq_loop_pid.out + self->uq_feedforward + self->in_uq_cmd, -1, 1);

        /* test */
        // static uint32 tick = 0;
        // tick++;

        // self->foc.ud = 0.0;
        // self->foc.uq = -0.1;
        // self->foc.theta = 0.0;

        /* ipark */
        self->foc.IPark(self->foc.ud,
                        self->foc.uq,
                        self->foc.theta,
                        &self->foc.ualpha,
                        &self->foc.ubeta);

        // self->out_u_A = self->foc.ualpha;
        // self->out_u_B = self->foc.ubeta;
    }
    // GPIO_WriteBit(GPIOB, GPIO_Pin_3, RESET);
    // ENABLE_GLOBAL_INTERRUPT();
}

void ThreeLoopPidFocV1_0_FhanLeso_Clear(struct ThreeLoopPidFocV1_0 *self)
{
    self->pos_loop_pid.Clear(&self->pos_loop_pid);
    self->spd_loop_pid.Clear(&self->spd_loop_pid);
    self->id_loop_pid.Clear(&self->id_loop_pid);
    self->iq_loop_pid.Clear(&self->iq_loop_pid);

    // self->spd_fdb_filter_moving_average.InitBuff(&self->spd_fdb_filter_moving_average, 0);
    self->speed_filter.Clear(&self->speed_filter);
    self->ialpha_filter.Clear(&self->ialpha_filter);
    self->ibeta_filter.Clear(&self->ibeta_filter);

    for (uint16 i = 0; i < FILTER_NOTCH_NUM; i++)
    {
        self->spd_out_filter_notch[i].Clear(&self->spd_out_filter_notch[i]);
    }
    self->spd_out_filter_binary_second_order.Clear(&self->spd_out_filter_binary_second_order);

    self->leso.Init_z1z2z3(&self->leso, self->in_pos_fdb, 0, 0);
}

void ThreeLoopPidFocV1_0_UpdateParam(struct ThreeLoopPidFocV1_0 *self)
{
    self->pos_loop_pid.param_T = self->param_pos_period;
    self->pos_loop_pid.ud_filter.param_Ts = self->param_pos_period;
    self->spd_loop_pid.param_T = self->param_pos_period;
    self->spd_loop_pid.ud_filter.param_Ts = self->param_pos_period;
    self->id_loop_pid.param_T = self->param_cur_period;
    self->id_loop_pid.ud_filter.param_Ts = self->param_cur_period;
    self->leso.param_h = self->param_pos_period;
    self->leso.td.param_h = self->param_pos_period;
    self->speed_filter.param_Ts = self->param_pos_period;
    self->ialpha_filter.param_Ts = self->param_cur_period;

    for (uint16_t i = 0; i < FILTER_NOTCH_NUM; i++)
    {
        self->spd_out_filter_notch[i].param_Ts = self->param_pos_period;
    }
    self->spd_out_filter_binary_second_order.param_Ts = self->param_pos_period;


    self->z3_filter_binary_second_order.param_Ts = self->param_pos_period;
    self->td.param_h = self->param_pos_period;

    self->iq_loop_pid = self->id_loop_pid;
    self->ibeta_filter = self->ialpha_filter;

    self->fwc.speed_source_ref = &self->in_spd_cmd;
    self->fwc.speed_source_fdb = &self->spd_fdb_ctrl;

    switch (self->param_control_algorithm)
    {
    case ControlAlgorithm_SeriesPidLeso:
        self->Run = ThreeLoopPidFocV1_0_SeriesPidLeso_Run;
        self->Clear = ThreeLoopPidFocV1_0_SeriesPidLeso_Clear;
        break;
    case ControlAlgorithm_ParallelPidLeso:
        // self->Run = ThreeLoopPidFocV1_0_ParallelPidLeso_Run;
        // self->Clear = ThreeLoopPidFocV1_0_ParallelPidLeso_Clear;
        break;
    case ControlAlgorithm_FhanLeso:
        // self->Run = ThreeLoopPidFocV1_0_FhanLeso_Run;
        // self->Clear = ThreeLoopPidFocV1_0_FhanLeso_Clear;
        break;
    default:
        self->Run = ThreeLoopPidFocV1_0_SeriesPidLeso_Run;
        self->Clear = ThreeLoopPidFocV1_0_SeriesPidLeso_Clear;
        break;
    }

    /* link pos_fdb */
    switch (self->param_pos_acquire_method)
    {
    case PosAcquireMethod_DirectFromEncoder:
        self->pos_fdb_ctrl_link = &self->in_pos_fdb;
        break;
    case PosAcquireMethod_LESO_z1:
        self->pos_fdb_ctrl_link = &self->leso.out_z1;
        break;
    default:
        self->pos_fdb_ctrl_link = &self->in_pos_fdb;
        break;
    }

    /* link spd_fdb */
    // self->spd_fdb_filter_moving_average.Run(&self->spd_fdb_filter_moving_average, self->in_spd_fdb);
    switch (self->param_spd_acquire_method)
    {
    case SpdAcquireMethod_DirectFromEncoder:
        self->spd_fdb_ctrl_link = &self->in_spd_fdb;
        break;
    case SpdAcquireMethod_MovingAverage:
        // self->spd_fdb_ctrl_link = &self->spd_fdb_filter_moving_average.out_y;
        break;
    case SpdAcquireMethod_FIR_FirstOrder:
        self->spd_fdb_ctrl_link = &self->speed_filter.out_y;
        break;
    case SpdAcquireMethod_LESO_z2:
        self->spd_fdb_ctrl_link = &self->leso.out_z2;
        break;
    default:
        self->spd_fdb_ctrl_link = &self->in_spd_fdb;
        break;
    }

    /* speed feedforward init */
    switch (self->param_speed_feedforward_mode)
    {
    case VoltageFeedforwardMode_SpdRef:
        self->speed_source = &self->in_spd_cmd;
        break;
    case VoltageFeedforwardMode_SpdFdb:
        self->speed_source = &self->spd_fdb_ctrl;
        break;
    default:
        break;
    }
}